#include "IntegralImage.h"
#include "Point.h"
#include "Plane.h"
#include "Line.h"

struct segment_pixel {
	int w;
	int h; 
	int r;
	int g;
	int b;
	float x;
	float y;
	float z;
	float rgb_edge;
	float depth_edge;
	float normal_edge;
	float combine_edge;
	bool valid_depth;
	int segment_id;
	segment_pixel * north;
	segment_pixel * south;
	segment_pixel * east;
	segment_pixel * west;
	//MyLinkedList<segment_pixel * > * segment;
	//MyLink<segment_pixel * > * link;
};

struct segmentation_fit {
	//MyLinkedList<segment_pixel * > * segment;
	Matrix3f covMat;
	Matrix3f U;
	Vector3f S;
	Matrix3f V;
	Vector3f mean;
	int nr_valid;
};

vector<Plane *> * segment(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud);
segmentation_fit * fit_surface(vector<float> * seg_x, vector<float> * seg_y, vector<float> * seg_z);

void segmentHybrid(vector<segmentation_fit * > * fit_segments,vector<vector<int> * > * selected_segments_w, vector<vector<int> * > * selected_segments_h, int ** segmented , vector<int> * segment_w, vector<int> * segment_h, float ** x,float ** y,float ** z, float ** edge, int & segment_id ,int iter, int width,int height, float min)
{
	int segment_counter	= segment_id+1;
	int offset 			= 25;
	int max_w 			= width-offset;
	int max_h 			= height-offset;
	
	for(unsigned int counter = 0; counter < segment_w->size(); counter++)
	{
		int i = segment_w->at(counter);
		int j = segment_h->at(counter);
		if(segmented[i][j] == segment_id && edge[i][j] < min){
			segmented[i][j] = segment_counter;
			vector<int>		  queue_w;
			vector<int>		  queue_h;
			vector<int>		* seg_w	= new vector<int>();
			vector<int>		* seg_h	= new vector<int>();
			vector<float>	* seg_x	= new vector<float>();
			vector<float>	* seg_y	= new vector<float>();
			vector<float>	* seg_z	= new vector<float>();
				
			queue_w.push_back(i);
			queue_h.push_back(j);
			while(!queue_w.empty()){
				int w_current = queue_w.back();
				int h_current = queue_h.back();
				//printf("w_current: %i, h_current:%i\n",w_current,h_current);
				queue_w.pop_back();
				queue_h.pop_back();
					
				seg_w->push_back(w_current);
				seg_h->push_back(h_current);
				seg_x->push_back(x[w_current][h_current]);
				seg_y->push_back(y[w_current][h_current]);
				seg_z->push_back(z[w_current][h_current]);

				int curr_min_w = w_current-1;
				if(curr_min_w < offset)		{curr_min_w = offset;}
				int curr_max_w = w_current+2;
				if(curr_max_w > max_w)	{curr_max_w = max_w;}
				
				int curr_min_h = h_current-1;
				if(curr_min_h < offset)		{curr_min_h = offset;}
				int curr_max_h = h_current+2;
				if(curr_min_h > max_h)	{curr_min_h = max_h;}
					
				for(int a = curr_min_w; a < curr_max_w; a++){
					for(int b = curr_min_h; b < curr_max_h; b++){
						if(segmented[a][b] == segment_id && edge[a][b] < min){
							segmented[a][b] = segment_counter;
							queue_w.push_back(a);
							queue_h.push_back(b);
						}
					}
				}
			}
				
			segmentation_fit * fit = fit_surface(seg_x, seg_y, seg_z);
			delete seg_x;
			delete seg_y;
			delete seg_z;
			
			if		(fit != 0 && fit->S(2)/sqrt(fit->S(0)) < 0.0002f){
				selected_segments_w->push_back(seg_w);
				selected_segments_h->push_back(seg_h);
				fit_segments->push_back(fit);
			}
			else if	(fit != 0 && min > 0.001){
				int  sid = segment_counter;
				segmentHybrid(fit_segments,selected_segments_w,selected_segments_h,segmented,seg_w,seg_h,x,y,z,edge,sid,iter,width,height,min*0.667f);
				segment_counter = sid;
				delete fit;
				delete seg_w;
				delete seg_h;
			}
			else{
				if(fit == 0){delete fit;}
				delete seg_w;
				delete seg_h;
			}
			segment_counter++;
		}
	}
}
using namespace std;
void segmentHybrid(vector<segmentation_fit * > * fit_segments, vector<vector<int> * > * selected_segments_w,vector<vector<int> * > * selected_segments_h,float ** edge, float ** x,float ** y,float ** z, int width,int height)
{

	int segment_id = -1;
	int ** segmented = new int*[width];
	for(int i = 0; i < width; i++){
		segmented[i] = new int[height];
		for(int j = 0; j < height; j++){
			segmented[i][j] = segment_id;
		}
	}

	float min 			= 0.5f;
	int offset 			= 25;
	int max_w 			= width-offset;
	int max_h 			= height-offset;
	
	vector<int> * seg_w			= new vector<int>();
	vector<int> * seg_h			= new vector<int>();
	

	for(int i = offset; i < max_w; i++)
	{
		for(int j = offset; j < max_h; j++)
		{
			seg_w->push_back(i);
			seg_h->push_back(j);
		}
	}
	segmentHybrid(fit_segments,selected_segments_w, selected_segments_h, segmented,seg_w,seg_h,x,y,z,edge,segment_id,segment_id,width,height,min);
	
	delete seg_w;
	delete seg_h;
	for(int i = 0; i < width; i++){
		delete[] segmented[i];
	}
	delete[] segmented;
};

struct MergePlanesRet {
	vector<vector<Point *> * > * 				p_vec;
	vector<vector<FeatureDescriptor *> * > * 	rgb_vec;
	vector<Plane *> * 							planes;
};

MergePlanesRet * mergePlanes(vector<segmentation_fit * > * fit_segs, vector<vector<int> * > * selected_segs_w,vector<vector<int> * > * selected_segs_h, float** x_arr,float** y_arr,float** z_arr,float** r_arr,float** g_arr,float** b_arr, int width,int height){

	vector<vector<Point *> * > * 				p_vec_all   = new vector<vector<Point *> * >();
	vector<vector<FeatureDescriptor *> * > * 	rgb_vec_all = new vector<vector<FeatureDescriptor *> * >();
	vector<Plane *> * 							planes		= new vector<Plane *>();
	MergePlanesRet * merged = new MergePlanesRet;
	merged->p_vec 			= p_vec_all;
	merged->rgb_vec 		= rgb_vec_all;
	merged->planes	 		= planes;
	vector<int> binsizes;
	binsizes.push_back(4);
	binsizes.push_back(4);
	binsizes.push_back(4);
	vector<float> max_val;
	max_val.push_back(1);
	max_val.push_back(1);
	max_val.push_back(1);
	vector<float> min_val;
	min_val.push_back(0);
	min_val.push_back(0);
	min_val.push_back(0);
	
	for(unsigned int i = 0; i < selected_segs_w->size(); i++)
	{
		vector<int> * segs_w = selected_segs_w->at(i);
		vector<int> * segs_h = selected_segs_h->at(i);
		
		vector<Point *> * p_vec = new vector<Point *>();
		vector<FeatureDescriptor *> * rgb_vec = new vector<FeatureDescriptor *>();
		for(unsigned int j = 0; j < segs_w->size(); j++)
		{
			int w = segs_w->at(j);
			int h = segs_h->at(j);
			float z = z_arr[w][h];
			if(z > 0 && !isnan(z)){
				float x = x_arr[w][h];
				float y = y_arr[w][h];
				p_vec->push_back(new Point(x, y, z, w, h));
			}
			rgb_vec->push_back(new RGBFeatureDescriptor(r_arr[w][h],g_arr[w][h],b_arr[w][h]));
		}
		Plane * plane 			= new Plane(p_vec);
		plane->color_histogram 	= new Histogram(rgb_vec,binsizes,max_val,min_val);
		//printf("new plane...\n");
		//plane->color_histogram->print();
		planes					->push_back(plane);
		rgb_vec_all				->push_back(rgb_vec);
		p_vec_all				->push_back(p_vec);
	}

	for(unsigned int i = 0; i < planes->size(); i++)
	{
		for(unsigned int j = i+1; j < planes->size(); j++)
		{
			Plane * plane_a 	= planes->at(i);
			Plane * plane_b 	= planes->at(j);
			float normal_angle 	= fabs(plane_a->normal_x*plane_b->normal_x+plane_a->normal_y*plane_b->normal_y+plane_a->normal_z*plane_b->normal_z);
			float hist_dist		= plane_a->color_histogram->distance(plane_b->color_histogram);
			if(hist_dist < 3.5f && normal_angle > 0.993)
			{
				vector<Point *> * p_vec_a = p_vec_all->at(i);
				vector<Point *> * p_vec_b = p_vec_all->at(j);
				vector<FeatureDescriptor *> * rgb_vec_a = rgb_vec_all->at(i);
				vector<FeatureDescriptor *> * rgb_vec_b = rgb_vec_all->at(j);
			
				Plane * large_plane;
				//Plane * small_plane;
				vector<Point *> * small_part;
				//vector<Point *> * large_part;
				
				if(p_vec_a->size() > p_vec_b->size()){
					large_plane = plane_a;
					//small_plane = plane_b;
					//large_part = p_vec_a;
					small_part = p_vec_b;
				}else{
					large_plane = plane_b;
					//small_plane = plane_a;
					//large_part = p_vec_b;
					small_part = p_vec_a;
				}
				
				float avg_square_dist = 0;
				for(unsigned int k = 0; k < small_part->size(); k++)
				{
					float p_dist = large_plane->distance(small_part->at(k));
					avg_square_dist += p_dist*p_dist;
				}
				avg_square_dist /= float(small_part->size());
			
				if(avg_square_dist < 0.0001){
					vector<Point *> * p_vec_new = new vector<Point *>();
					for(unsigned int k = 0; k < p_vec_a->size(); k++){p_vec_new->push_back(p_vec_a->at(k));}
					delete p_vec_a;
					for(unsigned int k = 0; k < p_vec_b->size(); k++){p_vec_new->push_back(p_vec_b->at(k));}
					delete p_vec_b;
				
					vector<FeatureDescriptor *> * new_rgb_vec = new vector<FeatureDescriptor *>();
					for(unsigned int k = 0; k < rgb_vec_a->size(); k++){new_rgb_vec->push_back(rgb_vec_a->at(k));}
					delete rgb_vec_a;
					for(unsigned int k = 0; k < rgb_vec_b->size(); k++){new_rgb_vec->push_back(rgb_vec_b->at(k));}
					delete rgb_vec_b;
				
					delete plane_a;
					delete plane_b;
				
					Plane * plane 			= new Plane(p_vec_new);
					plane->color_histogram 	= new Histogram(new_rgb_vec,binsizes,max_val,min_val);
				
					planes->at(i) = plane;
					planes->at(j) = planes->at(planes->size()-1);
					planes->pop_back();
				
					p_vec_all->at(i) = p_vec_new;
					p_vec_all->at(j) = p_vec_all->at(p_vec_all->size()-1);
					p_vec_all->pop_back();
					
					rgb_vec_all->at(i) = new_rgb_vec; 
					rgb_vec_all->at(j) = rgb_vec_all->at(rgb_vec_all->size()-1);
					rgb_vec_all->pop_back();
					j--;
				}
			}
		}
	}
	/*
	for(int i = 0; i < planes->size(); i++)
	{
		for(int j = i+1; j < planes->size(); j++)
		{
			Plane * plane_a 	= planes->at(i);
			Plane * plane_b 	= planes->at(j);
			float normal_angle 	= fabs(plane_a->normal_x*plane_b->normal_x+plane_a->normal_y*plane_b->normal_y+plane_a->normal_z*plane_b->normal_z);
			float hist_dist		= plane_a->color_histogram->distance(plane_b->color_histogram);
			if(hist_dist < 0.1 && normal_angle < 0.05)
			{
				Line * line = new Line(plane_a,plane_b);
				
				vector<Point *> * p_vec_a = p_vec_all->at(i);
				vector<Point *> * p_vec_b = p_vec_all->at(j);

				
				IplImage * rgb_img_clone = cvCreateImage(cvSize(rgb_img->width, rgb_img->height), IPL_DEPTH_8U, 3);
				cvCopy( rgb_img, rgb_img_clone, NULL );
				int counter_a = 0;
				for(int k = 0; k < p_vec_a->size(); k++)
				{
					int w = p_vec_a->at(k)->w;
					int h = p_vec_a->at(k)->h;
					if(line->distance(p_vec_a->at(k)) < 0.1){
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+2] = 255;
						counter_a++;
					}else{
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+1] = 255;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+2] = 0;
					}
				}
				printf("-----------------------------------\n");
				int counter_b = 0;
				for(int k = 0; k < p_vec_b->size(); k++)
				{
					int w = p_vec_b->at(k)->w;
					int h = p_vec_b->at(k)->h;
					if(line->distance(p_vec_b->at(k)) < 0.1){
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+0] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+2] = 255;
						counter_b++;
					}else{
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+0] = 255;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+1] = 0;
						rgb_img_clone->imageData[3*(h*rgb_img->width+w)+2] = 255;
					}
				}
				if(counter_b > 100 && counter_a > 100){
					cvNamedWindow("connect_segment", CV_WINDOW_AUTOSIZE );
					cvShowImage("connect_segment", rgb_img_clone);
					cvWaitKey(0);
				}
				cvReleaseImage( &rgb_img_clone );
			}
		}
	}
	*/
	return merged;
};

segmentation_fit * fit_surface(vector<float> * seg_x, vector<float> * seg_y, vector<float> * seg_z)
{
	float x_sum = 0;
	float y_sum = 0;
	float z_sum = 0;
	int nr_points = 0;
	for(unsigned int i = 0; i < seg_x->size(); i++)
	{
		float z = seg_z->at(i);
		if(z > 0 && !std::isnan(z))
		{
			nr_points++;
			x_sum+=seg_x->at(i);
			y_sum+=seg_y->at(i);
			z_sum+=z;
		}
	}
	
	if(nr_points > 1000)
	{
		x_sum/=(float)nr_points;
		y_sum/=(float)nr_points;
		z_sum/=(float)nr_points;
		
		segmentation_fit * fit = new segmentation_fit;
		fit->nr_valid = nr_points;
		fit->mean[0] = x_sum;
		fit->mean[1] = y_sum;
		fit->mean[2] = z_sum;
		
		float * x = new float[nr_points];
		float * y = new float[nr_points];
		float * z = new float[nr_points];
		std::vector<float *> data;
		data.push_back(x);
		data.push_back(y);
		data.push_back(z);
		int c = 0;
		for(unsigned int i = 0; i < seg_x->size(); i++){
			float zi = seg_z->at(i);
			if(zi > 0 && !std::isnan(zi)){
				x[c] = seg_x->at(i) - x_sum;
				y[c] = seg_y->at(i) - y_sum;
				z[c] = seg_z->at(i) - z_sum;
				c++;
			}
		}
				
		MatrixXf covMat(data.size(),data.size());

		for(unsigned int i = 0; i < data.size(); i++)
		{
			for(int unsigned j = i; j < data.size(); j++)
			{
				float * col1 	= data.at(i);
				float * col2 	= data.at(j);
				float sum = 0;
				for(int k = 0; k < nr_points; k++)
				{
					sum+=col1[k]*col2[k];
				}
				covMat(i,j)=sum/float(nr_points-1);
				covMat(j,i)=covMat(i,j);
			}
		}		
		fit->covMat = covMat;
		JacobiSVD<MatrixXf> svd(covMat, ComputeThinU | ComputeThinV);
		fit->U = svd.matrixU();
		fit->S = svd.singularValues();
		fit->V = svd.matrixV();
		//S/=S(0);
		delete[] x;
		delete[] y;
		delete[] z;
		return fit;
	}
	return 0;
}

void test(float** edge, int width,int height)
{
	IplImage * img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
	char * data 			= img->imageData;
	for(int j = 0; j < height; j++)
	{
		for(int i = 0; i < width; i++)
		{
			int ind = 3 * (j * width + i);
			data[ind + 0]=(char)(255.0f*edge[i][j]);
			data[ind + 1]=(char)(255.0f*edge[i][j]);
			data[ind + 2]=(char)(255.0f*edge[i][j]);
		}
	}
	
	int steps_w = 15;
	int steps_h = 15;
	int radius = 18;
	int radius_square = radius*radius;
	int * w_rad = new int[radius*(radius+1)*4];
	int * h_rad = new int[radius*(radius+1)*4];
	int counter = 0;
	for(int i = -radius; i <= radius; i++){
		for(int j = -radius; j <= radius; j++){
			if(radius_square > i*i + j*j){
				w_rad[counter] = i;
				h_rad[counter] = j;
				counter++;
			}
		}
	}
	for(int i = steps_w; i < width ; i+=steps_w){
		//cvLine(img,cvPoint(i, 0),cvPoint(i, height), cvScalar(0, 0, 255, 0), 1, 8, 0);  
	}
	for(int i = steps_h; i < height; i+=steps_h){
		//cvLine(img,cvPoint(0, i),cvPoint(width, i), cvScalar(0, 0, 255, 0), 1, 8, 0);
	}

	for(int i = 0; i < width; i+=steps_w){
		for(int j = 0; j < height; j+=steps_h){
			float sum_width = 0;
			float sum_height = 0;
			float sum_weight = 0;
			float end_w = i+steps_w;
			if(end_w > width){end_w = width;}
			float end_h = j+steps_h;
			if(end_h > height){end_h = height;}
			
			for(float w = i; w < end_w; w++){
				for(float h = j; h < end_h; h++){
					float val = edge[(int)w][(int)h];
					sum_width += w*val;
					sum_height+= h*val;
					sum_weight+= val;
				}
			}
			float mean_w = sum_width/sum_weight;
			float mean_h = sum_height/sum_weight;

			float dxx = 0;
			float dxy = 0;
			float dyy = 0;
			float valsum= 0;
			if(mean_w > radius && mean_h > radius && mean_w < width-radius-1 && mean_h < height-radius-1)
			{ 
				for(int c = 0; c < counter; c++){
					int w = w_rad[c]+(mean_w+0.5f);
					int h = h_rad[c]+(mean_h+0.5f);
					float val = edge[w][h];
					float w_diff = val*(float(w) - mean_w);
					float h_diff = val*(float(h) - mean_h);
					dxx += w_diff*w_diff;
					dxy += w_diff*h_diff;
					dyy += h_diff*h_diff;
					valsum+=val;
				}
			
				MatrixXf covMat(2,2);
				covMat(0,0) = dxx;
				covMat(1,1) = dyy;
				covMat(0,1) = dxy;
				covMat(1,0) = dxy;

				//JacobiSVD<MatrixXf> svd(covMat, ComputeThinU | ComputeThinV);
				//Matrix2f U = svd.matrixU();
				//Vector2f S = svd.singularValues();
				//Matrix2f V = svd.matrixV();
				
				//float len = S(0)*0.0005f;
				//float xv = U(0,0)*len;
				//float yv = U(1,0)*len;
			
				//cvLine(img,cvPoint(mean_w+xv,mean_h+yv),cvPoint(mean_w-xv,mean_h-yv), cvScalar(0, 0, 255, 0), 1, 8, 0);
			}
		}
	}
	//cvNamedWindow("edges", CV_WINDOW_AUTOSIZE );
	//cvShowImage("edges", img);
	//cvWaitKey(0);
	cvReleaseImage( &img );
}

void disp_edge(string s , float** edge, int width,int height)
{
	IplImage * img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	float * data 			= (float *)(img->imageData);
	float max = -10000000000;
	float min = 10000000000;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			if(edge[i][j] != -1){
				if(edge[i][j] < min){min = edge[i][j];}
				if(edge[i][j] > max){max = edge[i][j];}
			}
		}
	}
	min = 0;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			if(edge[i][j] != -1){
				data[(j * width + i)*3+0] = (edge[i][j]-min)/(max-min);
				data[(j * width + i)*3+1] = (edge[i][j]-min)/(max-min);
				data[(j * width + i)*3+2] = (edge[i][j]-min)/(max-min);
			}else{
				data[(j * width + i)*3+0] = 0;
				data[(j * width + i)*3+1] = 0;
				data[(j * width + i)*3+2] = 1;
			}
		}
	}
	//cvNamedWindow(s, CV_WINDOW_AUTOSIZE );
	//cvShowImage(s, img);
	//cvWaitKey(0);
	cvReleaseImage( &img );
}

const bool debugg_segment = true;
vector<Plane *> * segment(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud)
{
	int width 	= input_cloud->width;
	int height 	= input_cloud->height;
	//struct timeval start, end;
	//gettimeofday(&start, NULL);
	float ** norm_x 				= new float *[width];
	float ** norm_y 				= new float *[width];
	float ** norm_z 				= new float *[width];
	float ** x						= new float *[width];
	float ** y						= new float *[width];
	float ** z	 					= new float *[width];
	float ** r 						= new float *[width];
	float ** g 						= new float *[width];
	float ** b 						= new float *[width];
	float ** valid_depth 			= new float *[width];
	float ** valid_normal 			= new float *[width];
	pcl::PointXYZRGBNormal *** pointmat 	= new pcl::PointXYZRGBNormal **[width];
	for(int i = 0; i < width; i++)
	{
		norm_x[i]			= new float [height];
		norm_y[i]			= new float [height];
		norm_z[i]			= new float [height];
		
		x[i]				= new float [height];
		y[i]				= new float [height];
		z[i]				= new float [height]; 
		
		r[i]				= new float [height];
		g[i]				= new float [height];
		b[i]				= new float [height];
		valid_depth[i]		= new float [height];
		valid_normal[i]		= new float [height];
		pointmat[i]			= new pcl::PointXYZRGBNormal*[height];
	}
	//printf("w,h %i %i\n",width,height);
	//exit(0);
	int index = 0;
	float max_depth = -1;
	for(int j = 0; j < height; j++)
	{
		for(int i = 0; i < width; i++)
		{
			pcl::PointXYZRGBNormal point = input_cloud->points[index];
			pointmat[i][j] 		= &point;
			r[i][j]				=float(point.r)/255.0f;
			g[i][j]				=float(point.g)/255.0f;
			b[i][j]				=float(point.b)/255.0f;
			if(point.z > 0 && !std::isnan(point.z))	{
				valid_depth[i][j]	= 1;
				x[i][j]				= point.x;
				y[i][j]				= point.y;
				z[i][j]				= point.z;
				if(point.z > max_depth){max_depth = point.z;}
			}
			else{
				valid_depth[i][j]	= 0;
				z[i][j]			= 0;
			}
			if(!std::isnan(point.normal_x))	{
				valid_normal[i][j]	= 1;
				norm_x[i][j]		=point.normal_x;
				norm_y[i][j]		=point.normal_y;
				norm_z[i][j]		=point.normal_z;
			}
			else{
				valid_normal[i][j]	= 0;
				norm_x[i][j]		= 0;
				norm_y[i][j]		= 0;
				norm_z[i][j]		= 0;
			}
			index++;
		}
	}

	IntegralImage <float> ii_valid_depth;
	ii_valid_depth.setup(valid_depth,width,height);
	
	IntegralImage <float> ii_depth;
	ii_depth.setup(z,width,height);
	
	//float ** depth_edge 		= estimateEdge( r, 5 , width,height);//estimateDepthEdge( ii_depth, ii_valid_depth, 5 , width,	height);
	//float ** depth_edge 		= estimateDepthEdgev3( ii_depth, ii_valid_depth, 5 , width,	height);
	float ** depth_edge 		= estimate3DEdge( z, valid_depth,norm_x,norm_y,norm_z,valid_normal, width, height);
	disp_edge("Edge1", depth_edge, width,height);
	
	float depth_edge_max 	= -1;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			depth_edge[i][j] 	= sqrt(fabs(depth_edge[i][j]));
			if(depth_edge_max 	< depth_edge[i][j])	{depth_edge_max 	= depth_edge[i][j];	}	
		}
	}
	
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++){depth_edge[i][j]	/=depth_edge_max;	}
	}
	//test(depth_edge,width,height);
	//Extract segments
	vector<vector<int> * > * selected_segments_w	= new vector<vector<int> * >();
	vector<vector<int> * > * selected_segments_h	= new vector<vector<int> * >();
	vector<segmentation_fit * > * fit_segments		= new vector<segmentation_fit * >();
	
	segmentHybrid(fit_segments,selected_segments_w, selected_segments_h,depth_edge,x,y,z,width,height);
	
	
	MergePlanesRet * ret 	= mergePlanes(fit_segments,selected_segments_w, selected_segments_h,x,y,z,r,g,b,width,height);
	
	IplImage * img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	float * data 			= (float *)(img->imageData);

	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			data[(j * width + i)*3+0] = b[i][j];
			data[(j * width + i)*3+1] = g[i][j];
			data[(j * width + i)*3+2] = r[i][j];
		}
	}
	

	
	vector<vector<Point *> * > * 				p_vec = ret->p_vec;
	vector<vector<FeatureDescriptor *> * > * 	rgb_vec = ret->rgb_vec;
	vector<Plane *> * 							planes = ret->planes;
	
	for(unsigned int i = 0; i < p_vec->size(); i++)
	{
		float sr = float(rand() % 10000)/10000.0f;
		float sg = float(rand() % 10000)/10000.0f;
		float sb = float(rand() % 10000)/10000.0f;
		vector<Point *> * curr = p_vec->at(i);
		for(unsigned int j = 0; j < curr->size(); j++){
			int w = curr->at(j)->w;
			int h = curr->at(j)->h;
			data[(h * width + w)*3+0] = sb;
			data[(h * width + w)*3+1] = sg;
			data[(h * width + w)*3+2] = sr;
			delete curr->at(j);
		}
		delete curr;
	}
	delete p_vec;
	
	//cvNamedWindow("segmented", CV_WINDOW_AUTOSIZE );
	//cvShowImage("segmented", img);
	//cvWaitKey(0);
	cvReleaseImage( &img );
	
	for(unsigned int i = 0; i < rgb_vec->size(); i++)
	{
		vector<FeatureDescriptor *> * curr = rgb_vec->at(i);
		for(unsigned int j = 0; j < curr->size(); j++){delete curr->at(j);}
		delete curr;
	}
	delete rgb_vec;
	
	//for(unsigned int i = 0; i <planes->size(); i++){delete planes->at(i);}
	//delete planes;
	
	for(unsigned int i = 0; i < fit_segments->size(); i++){delete fit_segments->at(i);}
	delete fit_segments;
	
	for(unsigned int i = 0; i < selected_segments_w->size(); i++)
	{
		delete selected_segments_w->at(i);
		delete selected_segments_h->at(i);
	}
	delete selected_segments_w;
	delete selected_segments_h;

	ii_valid_depth.clear();
	ii_depth.clear();
	
	for(int i = 0; i < width; i++)
	{
		delete[] norm_x[i];
		delete[] norm_y[i];
		delete[] norm_z[i];
		delete[] x[i];
		delete[] y[i];
		delete[] z[i];
		delete[] r[i];
		delete[] g[i];
		delete[] b[i];
		delete[] valid_depth[i];
		delete[] valid_normal[i];
		delete[] pointmat[i];
		delete[] depth_edge[i];
	}
	delete[] norm_x;
	delete[] norm_y;
	delete[] norm_z;
	
	delete[] x;
	delete[] y;
	delete[] z;
	
	delete[] r;
	delete[] g;
	delete[] b;
	delete[] valid_depth;
	delete[] valid_normal;
	delete[] pointmat;
	delete[] depth_edge;
	return planes;
}
